#ifndef __UDSF_BBE_C_
#define __UDSF_BBE_C_

#include "udf_global.h"
/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "UDSF.c"形式引用
*/

static uint32_t udsf_get_psf(uint32_t tmp) __attribute__((unused));

static uint32_t udsf_get_psf(uint32_t tmp)
{
	uint8_t abuf_tmp_offset = 0;
	
	uint32_t bk = *(uint32_t*)(abuf_tmp_offset + 0x30000000);
	
	*(uint32_t*)(abuf_tmp_offset*2+0+0x30020000) = tmp;
	*(uint32_t*)(abuf_tmp_offset*2+4+0x30020000) = 0;
	tmp = *(uint32_t*)(abuf_tmp_offset + 0x30000000);
	
	*(uint32_t*)(abuf_tmp_offset + 0x30000000) = bk;
	return tmp;
}

static void udsf_rangeRefinement(STRUCT_POINTRST * pt,uint8_t range_inter_fac,uint32_t*pABS) __attribute__((unused));
static void udsf_rangeRefinement2(STRUCT_POINTRST * pt,uint8_t range_inter_fac,uint32_t*pABS) __attribute__((unused));

static void udsf_rangeRefinement(STRUCT_POINTRST * pt,uint8_t range_inner_factor,uint32_t*pABS)
{
    uint16_t d1Idx, d1LastIdx, d1NextIdx;
    uint8_t d2Idx;
    uint32_t d1CurrPow,d1LastPow, d1NextPow;
    int32_t valTmp1, valTmp2, valTmp3;
	
    if (pt->targetNum > 0)
    {
        for (uint16_t n = 0; n < pt->targetNum; n++)
        {
            d1Idx = pt->target[n].d1Idx;
            d2Idx = pt->target[n].d2Idx;
            d1LastIdx = ((d1Idx - 1) < 1) ? 2 : (d1Idx - 1);
            d1NextIdx = ((d1Idx + 1) >= UDFDEF_RANGE_USE) ? (UDFDEF_RANGE_USE - 1) : (d1Idx + 1);
            d1LastPow = pABS[d2Idx * UDFDEF_RANGE_USE + d1LastIdx];
            d1NextPow = pABS[d2Idx * UDFDEF_RANGE_USE + d1NextIdx];
            d1CurrPow = pABS[d2Idx * UDFDEF_RANGE_USE + d1Idx];
            if (((d1CurrPow >= d1LastPow) && (d1CurrPow > d1NextPow)) || ((d1CurrPow > d1LastPow) && (d1CurrPow >= d1NextPow)))
            {
                valTmp1 = (d1NextPow - d1LastPow) >> 1;
                valTmp1 = valTmp1 << range_inner_factor;
                valTmp2 = 2 * d1CurrPow - d1NextIdx - d1LastPow;
                valTmp3 = valTmp1 / valTmp2;
                pt->target[n].d1Idx = d1Idx * range_inner_factor + valTmp3;
            }
            else
            {
                pt->target[n].d1Idx = d1Idx * range_inner_factor;
            }
        }
    }
}

static void udsf_rangeRefinement2(STRUCT_POINTRST * pt,uint8_t range_inner_factor,uint32_t*pABS)
{
	for( int i=0;i<pt->targetNum;i++ ){
		
		uint32_t range_start,range_end;
		
		range_start = pt->target[i].d1Idx * range_inner_factor;
		pt->target[i].d1Idx = range_start;
		//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&range_start,1,"\nOrigin:","\n");
		
		if( range_start >= range_inner_factor ){
				range_end = range_start+1;
				range_start -= (range_inner_factor-1);
		}else{
			range_start = 0;
			range_end = (range_inner_factor-1);
		}

		//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&range_start,1,"\nstart:","\n");
		//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&range_end,1,"\nend:","\n");
		uint32_t max = 0;
		for( int j=range_start;j<range_end;j++ ){
			if( pABS[j] > max ){
				max = pABS[j];
				//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&j,1,"\nj:","\n");
				pt->target[i].d1Idx = j;
			}
		}
		
		//LOG_SEND_U16_BASE16(LOG_LEVEL_EMERG,1,&pt->target[i].d1Idx,1,"\nabs max:","\n");
	}
}
#endif
